/*******************************************************************************
* This file provides the functions for the Input Capture module.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "hal_ic.h"



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// Variables to store the RC pulse width information.
// Need to be declared as volatile as they will be modified by the ISR.
static volatile unsigned int prv_uiRcRx1 = 0;
static volatile unsigned int prv_uiRcRx2 = 0;
static volatile unsigned int prv_uiRcRx3 = 0;
static volatile unsigned int prv_uiRcRx4 = 0;
static volatile unsigned int prv_uiRcRx5 = 0;
static volatile unsigned int prv_uiRcRx6 = 0;
static volatile unsigned int prv_uiRcRx7 = 0;
static volatile unsigned int prv_uiRcRx8 = 0;

// Control mode.
static volatile enum MODE prv_eControlMode = MANUAL;

// Indicate there is new pulse from RC Rx.
// This will be set when new pulse arrive,
// and will be cleared by the device watchdog.
static volatile char cRcRxHeartbeat = 0;



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/





/*******************************************************************************
* PUBLIC FUNCTION: vInitIc
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initialize the Input Capture module 1 - 8.
*
*******************************************************************************/
void vInitIc(void)
{
	// Disable IC if enabled previously.
	CloseCapture1();
	CloseCapture2();
	CloseCapture3();
	CloseCapture4();
	CloseCapture5();
	CloseCapture6();
	CloseCapture7();
	CloseCapture8();
	
	
	
	// Configure IC interrupt.
	ConfigIntCapture1(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	ConfigIntCapture2(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	ConfigIntCapture3(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	ConfigIntCapture4(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	ConfigIntCapture5(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	ConfigIntCapture6(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	ConfigIntCapture7(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	ConfigIntCapture8(	IC_INT_ON &			// Interrupt enabled.
						IC_INT_PRIOR_6	);	// Interrupt priority 6.
	
	
	
	// Open the IC module.
	OpenCapture1(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
	
	OpenCapture2(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
	
	OpenCapture3(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
	
	OpenCapture4(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
	
	OpenCapture5(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
	
	OpenCapture6(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
	
	OpenCapture7(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
	
	OpenCapture8(	IC_IDLE_CON &		// Input capture continues to operate in CPU Idle mode.
					IC_TIMER3_SRC &		// TMR3 contents are captured on capture event.
					IC_INT_1CAPTURE&		// Interrupt on every capture event.
					IC_EVERY_EDGE	);	// Capture on every edge.
}



/*******************************************************************************
* PUBLIC FUNCTION: uiGetRcRx
*
* PARAMETERS:
* ~ ucChannel	- The RC Rx channel that we want to read.
*
* RETURN:
* ~ The RC Rx pulse width value in 0.2us step (Ex: 1.5ms = 7500). 
*
* DESCRIPTIONS:
* Get the RC Rx pulse width.
*
*******************************************************************************/
unsigned int uiGetRcRx(unsigned char ucChannel)
{
	unsigned int uiPulseWidth = 0;
	
	switch (ucChannel) {
		case 1:
			uiPulseWidth = prv_uiRcRx1;
			break;
		case 2:
			uiPulseWidth = prv_uiRcRx2;
			break;
		case 3:
			uiPulseWidth = prv_uiRcRx3;
			break;
		case 4:
			uiPulseWidth = prv_uiRcRx4;
			break;
		case 5:
			uiPulseWidth = prv_uiRcRx5;
			break;
		case 6:
			uiPulseWidth = prv_uiRcRx6;
			break;
		case 7:
			uiPulseWidth = prv_uiRcRx7;
			break;
		case 8:
			uiPulseWidth = prv_uiRcRx8;
			break;
	}
	
	return 	uiPulseWidth;
}	



/*******************************************************************************
* PUBLIC FUNCTION: eGetControlMode
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The current control mode.
*
* DESCRIPTIONS:
* Get the control mode determined by the switch on the RC Tx.
*
*******************************************************************************/
enum MODE eGetControlMode(void)
{
	return prv_eControlMode;
}	



/*******************************************************************************
* ISR: Interrupt for Input Capture 1.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC1Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS0bits.IC1IF = 0;
	
	// Read the captured value.
	ReadCapture1(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX1 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx1 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(1, prv_uiRcRx1);
		}	
	}
}	



/*******************************************************************************
* ISR: Interrupt for Input Capture 2.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC2Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS0bits.IC2IF = 0;
	
	// Read the captured value.
	ReadCapture2(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX2 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx2 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(2, prv_uiRcRx2);
		}	
	}
}	



/*******************************************************************************
* ISR: Interrupt for Input Capture 3.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC3Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS2bits.IC3IF = 0;
	
	// Read the captured value.
	ReadCapture3(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX3 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
		
		// Indicate there is new pulse.
		cRcRxHeartbeat = 1;
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx3 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(3, prv_uiRcRx3);
		}
	}
}	



/*******************************************************************************
* ISR: Interrupt for Input Capture 4.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC4Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS2bits.IC4IF = 0;
	
	// Read the captured value.
	ReadCapture4(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX4 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx4 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(4, prv_uiRcRx4);
		}
	}
}	



/*******************************************************************************
* ISR: Interrupt for Input Capture 5.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC5Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS2bits.IC5IF = 0;
	
	// Read the captured value.
	ReadCapture5(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX5 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx5 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(5, prv_uiRcRx5);
		}
		
		// Determine the control mode.
		// Make sure the pulse is valid.
		if (xSystemError.bRcRxError == 0) {
			if (prv_uiRcRx5 < PULSE_WIDTH_1500) {
				prv_eControlMode = MANUAL;
			}
			else {
				prv_eControlMode = STABILIZED;
			}
		}	
		
	}
}



/*******************************************************************************
* ISR: Interrupt for Input Capture 6.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC6Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS2bits.IC6IF = 0;
	
	// Read the captured value.
	ReadCapture6(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX6 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx6 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(6, prv_uiRcRx6);
		}
	}
}



/*******************************************************************************
* ISR: Interrupt for Input Capture 7.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC7Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS1bits.IC7IF = 0;
	
	// Read the captured value.
	ReadCapture7(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX7 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx7 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
//		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(7, prv_uiRcRx7);
//		}
	}
}



/*******************************************************************************
* ISR: Interrupt for Input Capture 8.
*
* DESCRIPTIONS:
* Calculate the pulse width of the pwm signal.
*
*******************************************************************************/
void __attribute__((interrupt, no_auto_psv)) _IC8Interrupt(void)
{
	static unsigned int uiRisingEdge;
	static unsigned int uiFallingEdge;
	unsigned int uiCaptureBuffer[4] = {0};
	
	
	
	// Clear the interrupt flag.
	IFS1bits.IC8IF = 0;
	
	// Read the captured value.
	ReadCapture8(uiCaptureBuffer);
	
	
	
	// If the pin is high, that means we captured the rising edge.
	if (RCRX8 == 1) {	
		uiRisingEdge = uiCaptureBuffer[0];
	}
	
	// If the pin is low, then we captured the falling edge.
	else {
		uiFallingEdge = uiCaptureBuffer[0];
		
		// Calculate the pulse width.
		// Make sure the pulse width is valid.
		unsigned int uiPulseWidth = uiFallingEdge - uiRisingEdge;
		if ((uiPulseWidth > PULSE_WIDTH_500) && (uiPulseWidth < PULSE_WIDTH_2500)) {
			prv_uiRcRx8 = uiPulseWidth;
		}
		
		// Propagate the signal in manual mode.
		// Make sure the pulse is valid.
//		if ((eGetControlMode() == MANUAL) && (xSystemError.bRcRxError == 0)) {
			vUpdateServo(8, prv_uiRcRx8);
//		}
	}
}



/*******************************************************************************
* TASK: taskRcRxWatchdog
*
* DESCRIPTIONS:
* This task monitors the pulse from the RC Receiver. If no pulse is detected
* after the timeout period, the error flag will be set.
*
*******************************************************************************/
portTASK_FUNCTION(taskRcRxWatchdog, pvParameters)
{
	while (1) {
		// Reset the RC Rx state.
		cRcRxHeartbeat = 0;
		
		// Delay for 100ms.
		vTaskDelay(configTICK_RATE_HZ / 10);
		
		// Check the RC Rx state again.
		// Set the error flag if no new pulse received.
		if (cRcRxHeartbeat == 0) {
			xSystemError.bRcRxError = 1;
		}	
		else {
			// Wait a while so that the signal is stable if the signal is lost previously.
			if (xSystemError.bRcRxError == 1) {
				// Delay for 100ms.
				vTaskDelay(configTICK_RATE_HZ / 10);
			}
				
			xSystemError.bRcRxError = 0;
		}
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiRcRxWatchdog = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiRcRxWatchdog < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
	}	
}	
